package ch.ethz.fcl.metrobuzz.spatialindex.RTree;

import gnu.trove.map.hash.TIntObjectHashMap;
import gnu.trove.stack.array.TIntArrayStack;

import java.util.ArrayList;

import ch.ethz.fcl.metrobuzz.spatialindex.ISpatialIndex;
import ch.ethz.fcl.metrobuzz.spatialindex.object3d.BoundingBox;
import ch.ethz.fcl.metrobuzz.spatialindex.object3d.IObject3D;
import ch.ethz.fcl.metrobuzz.tools.MBTools;

public class RTreeSpatialIndex implements ISpatialIndex {
	int maxNodeEntries = 5000000;
	int minNodeEntries = 2500000;

	// map of nodeId -> node object
	private TIntObjectHashMap<RTreeNode> treeNodeMap = new TIntObjectHashMap<RTreeNode>();

	// set to true if debugging tree corruption
	private final static boolean INTERNAL_CONSISTENCY_CHECKING = false;

	// used to mark the status of entries during a node split
	private final static int ENTRY_STATUS_ASSIGNED = 0;
	private final static int ENTRY_STATUS_UNASSIGNED = 1;

	private byte[] entryStatus = null;
	private byte[] initialEntryStatus = null;

	// stacks used to store nodeId and entry index of each node from the root
	// down
	// to the leaf. enables fast lookup of nodes when a split is propagated up
	// the tree.
	private TIntArrayStack parents = new TIntArrayStack();
	private TIntArrayStack parentsEntry = new TIntArrayStack();

	// Initialization
	private int treeHeight = 1;
	private int rootNodeId = 0;
	private int size = 0;

	// enables creation of new nodes
	private int highestUsedNodeId = rootNodeId;

	// deleted node objects are retained in the nodeMap,
	// so that they can be reused.
	private TIntArrayStack deletedNodeIds = new TIntArrayStack();
	public boolean initialized = false;

	@Override
	public BoundingBox getBoundingBox() {
		RTreeNode rootNode = getNode(rootNodeId);
		return getMBB(rootNode);
	}

	@Override
	public void init() {
		// The MinNodeEntries must be less than or equal to (int)
		// (MaxNodeEntries / 2)
		if (minNodeEntries < 1 || minNodeEntries > maxNodeEntries / 2) {
			minNodeEntries = maxNodeEntries / 2;
		}

		entryStatus = new byte[maxNodeEntries];
		initialEntryStatus = new byte[maxNodeEntries];

		for (int i = 0; i < maxNodeEntries; i++) {
			initialEntryStatus[i] = ENTRY_STATUS_UNASSIGNED;
		}

		treeHeight = 1;
		rootNodeId = 0;
		size = 0;

		RTreeNode root = new RTreeNode(rootNodeId, 1, maxNodeEntries);
		treeNodeMap.put(rootNodeId, root);
	}

	@Override
	public void add(IObject3D object, int id) {
		add(object, id, 1);
		size++;

		if (INTERNAL_CONSISTENCY_CHECKING) {
			checkConsistency();
		}
	}

	private void add(IObject3D object, int id, int level) {
		// I1 [Find position for new record] Invoke ChooseLeaf to select a
		// leaf node L in which to place r
		RTreeNode n = choose_leaf(object, level);
		RTreeNode newLeaf = null;

		// I2 [Add record to leaf node] If L has room for another entry,
		// install E. Otherwise invoke SplitNode to obtain L and LL containing
		// E and all the old entries of L
		if (n.entryCount < maxNodeEntries) {
			n.addEntry(object.getBoundingBox(), id);
		} else
			newLeaf = splitNode(n, object, id); // split node n

		// I3 [Propagate changes upwards] Invoke AdjustTree on L, also passing
		// LL
		// if a split was performed
		RTreeNode newNode = adjustTree(n, newLeaf);

		// I4 [Grow tree taller] If node split propagation caused the root to
		// split, create a new root whose children are the two resulting nodes.
		if (newNode != null) {
			int oldRootNodeId = rootNodeId;
			RTreeNode oldRoot = getNode(oldRootNodeId);

			rootNodeId = getNextNodeId();
			treeHeight++;
			RTreeNode root = new RTreeNode(rootNodeId, treeHeight,
					maxNodeEntries);
			root.addEntry(newNode.getBoundingBox(), newNode.nodeId);
			root.addEntry(oldRoot.getBoundingBox(), oldRoot.nodeId);
			treeNodeMap.put(rootNodeId, root);
		}
	}

	private RTreeNode choose_leaf(IObject3D object, int level) {
		RTreeNode n = getNode(rootNodeId);
		parents.clear();
		parentsEntry.clear();

		if (n == null) {
			System.out.println("Couldn't get root node (" + rootNodeId
					+ ")");
			return null;
		}

		// Leaf check
		while (true) {
			if (n.level == level)
				return n;

			// Choose subtree
			float leastEnlargement = BoundingBox.enlargement(
					n.entries[0].getBoundingBox(), object.getBoundingBox());

			int index = 0;
			for (int i = 1; i < n.entryCount; i++) {
				BoundingBox tempBB = new BoundingBox();
				tempBB.copy(n.entries[i].getBoundingBox());

				float tempEnlargement = BoundingBox.enlargement(tempBB,
						object.getBoundingBox());

				if ((tempEnlargement < leastEnlargement)
						|| ((tempEnlargement == leastEnlargement) && tempBB
								.volume() < n.entries[i].getBoundingBox()
								.volume())) {
					index = i;
					leastEnlargement = tempEnlargement;
				}
			}

			MBTools.debug("n ids index " + n.ids[index] + " " + index
					+ " sub nodes " + n.entryCount, false);
			
			parents.push(n.nodeId);
			parentsEntry.push(index);
			
			// Descend until a leaf is reached
			n = getNode(n.ids[index]);
		}
	}

	private RTreeNode splitNode(RTreeNode n, IObject3D object, int id) {
		// pick first entry for each group
		RTreeNode newNode = new RTreeNode(getNextNodeId(), n.level,
				maxNodeEntries);
		treeNodeMap.put(newNode.nodeId, newNode);

		System.arraycopy(initialEntryStatus, 0, entryStatus, 0, maxNodeEntries);
		pickSeeds(n, object, id, newNode);

		// check if done
		while (n.entryCount + newNode.entryCount < maxNodeEntries + 1) {
			if (maxNodeEntries + 1 - newNode.entryCount == minNodeEntries) {
				// assign all remaining entries to original node
				for (int i = 0; i < maxNodeEntries; i++) {
					if (entryStatus[i] == ENTRY_STATUS_UNASSIGNED) {
						entryStatus[i] = ENTRY_STATUS_ASSIGNED;

						n.bb.setMinX(Math.min(n.bb.getMinX(), n.entries[i]
								.getBoundingBox().getMinX()));
						n.bb.setMinY(Math.min(n.bb.getMinY(), n.entries[i]
								.getBoundingBox().getMinY()));
						n.bb.setMinZ(Math.min(n.bb.getMinZ(), n.entries[i]
								.getBoundingBox().getMinZ()));
						n.bb.setMaxX(Math.max(n.bb.getMaxX(), n.entries[i]
								.getBoundingBox().getMaxX()));
						n.bb.setMaxY(Math.max(n.bb.getMaxY(), n.entries[i]
								.getBoundingBox().getMaxY()));
						n.bb.setMaxZ(Math.max(n.bb.getMaxZ(), n.entries[i]
								.getBoundingBox().getMaxZ()));

						n.entryCount++;
					}
				}
				break;
			}
			if (maxNodeEntries + 1 - n.entryCount == minNodeEntries) {
				for (int i = 0; i < maxNodeEntries; i++) {
					if (entryStatus[i] == ENTRY_STATUS_UNASSIGNED) {
						entryStatus[i] = ENTRY_STATUS_ASSIGNED;
						newNode.addEntry(n.entries[i], n.ids[i]);
						n.ids[i] = -1;
					}
				}
				break;
			}
			// select entry to assign
			pickNext(n, newNode);
		}
		n.reorganize(this);

		if (INTERNAL_CONSISTENCY_CHECKING) {
			BoundingBox nMBB = new BoundingBox(n.bb.getMinX(), n.bb.getMinY(),
					n.bb.getMinZ(), n.bb.getMaxX(), n.bb.getMaxY(),
					n.bb.getMaxZ());
			if (nMBB.equals(getMBB(n)))
				System.out.println("Error: splitNode old node MBB wrong");
			BoundingBox newNodeMBB = new BoundingBox(newNode.bb.getMinX(),
					newNode.bb.getMinY(), newNode.bb.getMinZ(),
					newNode.bb.getMaxX(), newNode.bb.getMaxY(),
					newNode.bb.getMaxZ());
			if (newNodeMBB.equals(getMBB(newNode)))
				System.out.println("Error: spliteNode new node MBR wrong");
		}

		return newNode;
	}

	private void pickSeeds(RTreeNode n, IObject3D object, int newid,
			RTreeNode newNode) {
		float maxNormalizedSeparation = -1;
		int highestLowIndex = -1;
		int lowestHighIndex = -1;

		n.bb.setMinX(Math.min(n.bb.getMinX(), object.getBoundingBox().getMinX()));
		n.bb.setMinY(Math.min(n.bb.getMinY(), object.getBoundingBox().getMinY()));
		n.bb.setMinZ(Math.min(n.bb.getMinZ(), object.getBoundingBox().getMinZ()));
		n.bb.setMaxX(Math.max(n.bb.getMaxX(), object.getBoundingBox().getMaxX()));
		n.bb.setMaxY(Math.max(n.bb.getMaxY(), object.getBoundingBox().getMaxY()));
		n.bb.setMaxZ(Math.max(n.bb.getMaxZ(), object.getBoundingBox().getMaxZ()));

		float mbbLenX = n.bb.getMaxX() - n.bb.getMinX();
		float mbbLenY = n.bb.getMaxY() - n.bb.getMinY();
		float mbbLenZ = n.bb.getMaxZ() - n.bb.getMinZ();

		float tempHighestLow = object.getBoundingBox().getMinX();
		int tempHighestLowIndex = -1;

		float tempLowestHigh = object.getBoundingBox().getMaxX();
		int tempLowestHighIndex = -1;

		int sub_count = 0;
		if(n.isLeaf())
			sub_count = n.entryCount;
		else 
			sub_count = n.entryCount;
		
		for (int i = 0; i < sub_count; i++) {
			float tempLow = n.entries[i].getMinX();
			if (tempLow >= tempHighestLow) {
				tempHighestLow = tempLow;
				tempHighestLowIndex = i;
			} else {
				float tempHigh = n.entries[i].getMaxX();
				if (tempHigh <= tempLowestHigh) {
					tempLowestHigh = tempHigh;
					tempLowestHighIndex = i;
				}
			}
		}

		// Adjust for shape of the rectangle cluster. Normalize the separations
		// by dividing
		// by the widths of the entire set along the corresponding dimension
		float normalizedSeparation = mbbLenX == 0 ? 1
				: (tempHighestLow - tempLowestHigh) / mbbLenX;
		if (normalizedSeparation > 1 || normalizedSeparation < -1) {
			MBTools.debug("Invalid normalize separation X", false);
		}

		// Select the most extreme pair
		if (normalizedSeparation >= maxNormalizedSeparation) {
			highestLowIndex = tempHighestLowIndex;
			lowestHighIndex = tempLowestHighIndex;
			maxNormalizedSeparation = normalizedSeparation;
		}

		// repeat for the Y dimension
		tempHighestLow = object.getBoundingBox().getMinY();
		tempHighestLowIndex = -1;

		tempLowestHigh = object.getBoundingBox().getMaxY();
		tempLowestHighIndex = -1;

		for (int i = 0; i < sub_count; i++) {
			float tempLow = n.entries[i].getMinY();
			if (tempLow >= tempHighestLow) {
				tempHighestLow = tempLow;
				tempHighestLowIndex = i;
			} else {
				float tempHigh = n.entries[i].getMaxY();
				if (tempHigh <= tempLowestHigh) {
					tempLowestHigh = tempHigh;
					tempLowestHighIndex = i;
				}
			}
		}

		normalizedSeparation = mbbLenY == 0 ? 1
				: (tempHighestLow - tempLowestHigh) / mbbLenY;
		if (normalizedSeparation > 1 || normalizedSeparation < -1) {
			MBTools.debug("Invalid normalize separation Y", false);
		}

		// Select the most extreme pair
		if (normalizedSeparation >= maxNormalizedSeparation) {
			highestLowIndex = tempHighestLowIndex;
			lowestHighIndex = tempLowestHighIndex;
			maxNormalizedSeparation = normalizedSeparation;
		}

		// repeat for the Z dimension
		tempHighestLow = object.getBoundingBox().getMinZ();
		tempHighestLowIndex = -1;

		tempLowestHigh = object.getBoundingBox().getMaxZ();
		tempLowestHighIndex = -1;

		for (int i = 0; i < sub_count; i++) {
			float tempLow = n.entries[i].getMinZ();
			if (tempLow >= tempHighestLow) {
				tempHighestLow = tempLow;
				tempHighestLowIndex = i;
			} else {
				float tempHigh = n.entries[i].getMaxZ();
				if (tempHigh <= tempLowestHigh) {
					tempLowestHigh = tempHigh;
					tempLowestHighIndex = i;
				}
			}
		}

		normalizedSeparation = mbbLenZ == 0 ? 1
				: (tempHighestLow - tempLowestHigh) / mbbLenZ;
		if (normalizedSeparation > 1 || normalizedSeparation < -1) {
			MBTools.debug("Invalid normalize separation Z", false);
		}

		// Select the most extreme pair
		if (normalizedSeparation >= maxNormalizedSeparation) {
			highestLowIndex = tempHighestLowIndex;
			lowestHighIndex = tempLowestHighIndex;
			maxNormalizedSeparation = normalizedSeparation;
		}

		if (highestLowIndex == lowestHighIndex) {
			highestLowIndex = -1;
			float tempMinY = object.getBoundingBox().getMinY();
			lowestHighIndex = 0;
			float tempMaxX = n.entries[0].getMaxX();

			for (int i = 1; i < sub_count; i++) {
				if (n.entries[i].getMinY() < tempMinY) {
					tempMinY = n.entries[i].getMinY();
					highestLowIndex = i;
				} else if (n.entries[i].getMaxX() > tempMaxX) {
					tempMaxX = n.entries[i].getMaxX();
					lowestHighIndex = i;
				}
			}
		}

		if (highestLowIndex == -1)
			newNode.addEntry(object.getBoundingBox(), newid);
		else {
			newNode.addEntry(n.entries[highestLowIndex], n.ids[highestLowIndex]);
			n.ids[highestLowIndex] = -1;
			n.entries[highestLowIndex] = object.getBoundingBox();
			n.ids[highestLowIndex] = newid;
		}

		if (lowestHighIndex == -1)
			lowestHighIndex = highestLowIndex;

		entryStatus[lowestHighIndex] = ENTRY_STATUS_ASSIGNED;
		n.entryCount = 1;
		n.bb.copy(n.entries[lowestHighIndex]);
	}

	private RTreeNode adjustTree(RTreeNode n, RTreeNode nn) {
		while (n.level != treeHeight) {
			RTreeNode parent = getNode(parents.pop());
			int entry = parentsEntry.pop();

			if (parent.ids[entry] != n.nodeId) {
				System.out.println("Error: entry " + entry + " in node "
						+ parent.nodeId + " should point to node " + n.nodeId
						+ "; actually points to node " + parent.ids[entry]);
			}

			if (parent.entries[entry].getBoundingBox().getMinX() != n.bb
					.getMinX()
					|| parent.entries[entry].getBoundingBox().getMinY() != n.bb
							.getMinY()
					|| parent.entries[entry].getBoundingBox().getMinZ() != n.bb
							.getMinZ()
					|| parent.entries[entry].getBoundingBox().getMaxX() != n.bb
							.getMaxX()
					|| parent.entries[entry].getBoundingBox().getMaxY() != n.bb
							.getMaxY()
					|| parent.entries[entry].getBoundingBox().getMaxZ() != n.bb
							.getMaxZ()) {
				parent.entries[entry].getBoundingBox().copy(n.bb);

				parent.recalculateMBB();
			}
			RTreeNode newNode = null;
			if (nn != null) {
				if (parent.entryCount < maxNodeEntries)
					parent.addEntry(nn.getBoundingBox(), nn.nodeId);
				else{
					MBTools.debug("parents level "+parent.level, false);
					newNode = splitNode(parent, nn.bb, nn.nodeId);
					}
			}

			n = parent;
			nn = newNode;

			parent = null;
			newNode = null;
		}
		return nn;
	}

	private int pickNext(RTreeNode n, RTreeNode newNode) {
		float maxDifference = Float.NEGATIVE_INFINITY;
		int next = 0;
		int nextGroup = 0;

		for (int i = 0; i < maxNodeEntries; i++) {
			if (entryStatus[i] == ENTRY_STATUS_UNASSIGNED) {
				if (n.ids[i] == -1) {
					System.out.println("Error: Node " + n.nodeId + ", entry "
							+ i + " is null");
				}

				float nIncrease = BoundingBox.enlargement(n.bb,
						n.entries[i]);

				float newNodeIncrease = BoundingBox.enlargement(newNode.bb,
						n.entries[i]);
				float difference = Math.abs(nIncrease - newNodeIncrease);

				if (difference > maxDifference) {
					next = i;

					if (nIncrease < newNodeIncrease)
						nextGroup = 0;
					else if (newNodeIncrease < nIncrease)
						nextGroup = 1;
					else if (n.bb.volume() < newNode.bb.volume())
						nextGroup = 0;
					else if (n.bb.volume() > newNode.bb.volume())
						nextGroup = 1;
					else if (newNode.entryCount < maxNodeEntries / 2)
						nextGroup = 0;
					else
						nextGroup = 1;
					maxDifference = difference;
				}
			}
		}

		entryStatus[next] = ENTRY_STATUS_ASSIGNED;

		if (nextGroup == 0) {
			n.bb.setMinX(Math.min(n.entries[next].getMinX(),
					n.bb.getMinX()));
			n.bb.setMinY(Math.min(n.entries[next].getMinY(),
					n.bb.getMinY()));
			n.bb.setMinZ(Math.min(n.entries[next].getMinZ(),
					n.bb.getMinZ()));
			n.bb.setMaxX(Math.max(n.entries[next].getMaxX(),
					n.bb.getMinX()));
			n.bb.setMaxY(Math.max(n.entries[next].getMaxY(),
					n.bb.getMaxY()));
			n.bb.setMaxZ(Math.max(n.entries[next].getMaxZ(),
					n.bb.getMaxZ()));
			n.entryCount++;
		} else {
			if (n.isLeaf())
				newNode.addEntry(n.entries[next], n.ids[next]);
			else
				newNode.addEntry(n.entries[next], n.ids[next]);
			n.ids[next] = -1;
		}
		return next;
	}

	private BoundingBox getMBB(RTreeNode n) {
		return n.getBoundingBox();
	}

	public boolean checkConsistency() {
		return checkConsistency(rootNodeId, treeHeight, null);
	}

	private boolean checkConsistency(int nodeId, int expectedLevel,
			BoundingBox expectedMBB) {
		RTreeNode n = getNode(nodeId);

		if (n == null) {
			System.out.println("Error: Could not read node " + nodeId);
			return false;
		}

		if (nodeId == rootNodeId && size() == 0) {
			if (n.level != 1) {
				System.out
						.println("Error: tree is empty but root node is not at level 1");
				return false;
			}
		}

		if (n.level != expectedLevel) {
			System.out.println("Error: Node " + nodeId + ", expected level "
					+ expectedLevel + ", actual level " + n.level);
			return false;
		}

		BoundingBox calculatedMBB = getMBB(n);
		BoundingBox actualMBB = new BoundingBox();
		actualMBB.copy(n.bb);

		if (!actualMBB.equals(calculatedMBB)) {
			System.out.println("Error: Node " + nodeId
					+ ", calculated MBB does not equal stored MBB");
			if (actualMBB.getMinX() != n.bb.getMinX())
				System.out.println("  actualMinX=" + actualMBB.getMinX()
						+ ", calc=" + calculatedMBB.getMinX());
			if (actualMBB.getMinY() != n.bb.getMinY())
				System.out.println("  actualMinY=" + actualMBB.getMinY()
						+ ", calc=" + calculatedMBB.getMinY());
			if (actualMBB.getMinZ() != n.bb.getMinZ())
				System.out.println("  actualMinZ=" + actualMBB.getMinZ()
						+ ", calc=" + calculatedMBB.getMinZ());
			if (actualMBB.getMaxX() != n.bb.getMaxX())
				System.out.println("  actualMaxX=" + actualMBB.getMaxX()
						+ ", calc=" + calculatedMBB.getMaxX());
			if (actualMBB.getMaxY() != n.bb.getMaxY())
				System.out.println("  actualMaxY=" + actualMBB.getMaxY()
						+ ", calc=" + calculatedMBB.getMaxY());
			if (actualMBB.getMaxZ() != n.bb.getMaxZ())
				System.out.println("  actualMaxZ=" + actualMBB.getMaxZ()
						+ ", calc=" + calculatedMBB.getMaxZ());

			return false;
		}
		if (expectedMBB != null && !actualMBB.equals(expectedMBB)) {
			System.out.println("Error: Node " + nodeId
					+ ", expected MBB (from parent) does not equal stored MBB");
			return false;
		}

		for (int i = 0; i < n.entryCount; i++) {
			if (n.ids[i] == -1) {
				System.out.println("Error: Node " + nodeId + ", Entry " + i
						+ " is null");
				return false;
			}

			if (n.level > 1) {
				if (!checkConsistency(n.ids[i], n.level - 1,
						n.entries[i].getBoundingBox()))
					return false;
			}
		}
		return true;
	}

	private int getNextNodeId() {
		int nextNodeId = 0;
		if (deletedNodeIds.size() > 0)
			nextNodeId = deletedNodeIds.pop();
		else
			nextNodeId = 1 + highestUsedNodeId++;
		return nextNodeId;
	}

	public RTreeNode getNode(int id) {
		return (RTreeNode) treeNodeMap.get(id);
	}

	public int height() {
		return treeHeight;
	}

	@Override
	public int size() {
		return size;
	}

	@Override
	public boolean delete(IObject3D object, int id) {
		// parents.reset();
		parents.push(rootNodeId);

		// parentsEntry.reset();
		parentsEntry.push(-1);
		RTreeNode n = null;
		int foundIndex = -1;

		while (foundIndex == -1 && parents.size() > 0) {
			n = getNode(parents.peek());
			int startIndex = parentsEntry.peek() + 1;

			if (!n.isLeaf()) {
				System.out.println("Searching node " + n.nodeId
						+ " , from index " + startIndex);
				boolean contains = false;
				for (int i = startIndex; i < n.entryCount; i++) {
					if (n.entries[i].getBoundingBox().contains(
							object.getBoundingBox()))
						if (n.bb.contains(object.getBoundingBox())) {
							parents.push(n.ids[i]);
							parentsEntry.pop();
							parentsEntry.push(i);
							parentsEntry.push(-1);
							contains = true;
							break;
						}
				}
				if (contains) {
					continue;
				}
			} else
				foundIndex = n.findEntry(object.getBoundingBox(), id);

			parents.pop();
			parentsEntry.pop();
		}

		if (foundIndex != -1) {
			n.deleteEntry(foundIndex);
			condenseTree(n);
			size--;
		}

		RTreeNode root = getNode(rootNodeId);
		while (root.entryCount == 1 && treeHeight > 1) {
			deletedNodeIds.push(rootNodeId);
			root.entryCount = 0;
			rootNodeId = root.ids[0];
			treeHeight--;
			root = getNode(rootNodeId);
		}

		if (size == 0) {
			root.bb.setMinX(Float.MAX_VALUE);
			root.bb.setMinY(Float.MAX_VALUE);
			root.bb.setMinZ(Float.MAX_VALUE);
			root.bb.setMaxX(-Float.MAX_VALUE);
			root.bb.setMaxY(-Float.MAX_VALUE);
			root.bb.setMaxZ(-Float.MAX_VALUE);
		}

		if (INTERNAL_CONSISTENCY_CHECKING) {
			checkConsistency();
		}

		return (foundIndex != -1);
	}

	private void condenseTree(RTreeNode l) {
		RTreeNode n = l;
		RTreeNode parent = null;
		int parentEntry = 0;

		TIntArrayStack eliminatedNodeIds = new TIntArrayStack();

		while (n.level != treeHeight) {
			parent = getNode(parents.pop());
			parentEntry = parentsEntry.pop();

			if (n.entryCount < minNodeEntries) {
				parent.deleteEntry(parentEntry);
				eliminatedNodeIds.push(n.nodeId);
			} else {
				if (!n.equals(n.entries[parentEntry].getBoundingBox())) {
					BoundingBox deletedBB = new BoundingBox();
					deletedBB
							.copy(parent.entries[parentEntry].getBoundingBox());
					parent.recalculateMBBIfInfluencedBy(deletedBB);
				}
			}
			n = parent;
		}
		while (eliminatedNodeIds.size() > 0) {
			RTreeNode e = getNode(eliminatedNodeIds.pop());
			for (int j = 0; j < e.entryCount; j++) {
				add(e.entries[j], e.ids[j], e.level);
				e.ids[j] = -1;
			}
			e.entryCount = 0;
			deletedNodeIds.push(e.nodeId);
		}
	}

	// @param: level
	// get the nodes at level
	public ArrayList<BoundingBox> getLevel(int level) {
		ArrayList<BoundingBox> nodes = new ArrayList<BoundingBox>();

		parents.clear();
		parents.push(rootNodeId);
		while (parents.size() > 0) {
			RTreeNode n = getNode(parents.pop());

			MBTools.debug(n.entryCount + " entries ", false);
			if (n.getLevel() == level)
				nodes.add(getMBB(n));

			else if (n.getLevel() > level)
				for (int i = 0; i < n.entryCount; i++)
					parents.push(n.ids[i]);
		}

		MBTools.debug("Level " + level + " Node size " + nodes.size(), false);
		return nodes;
	}

	// @param: level
	// get the nodes at level
	public ArrayList<ArrayList<Integer>> getSubNodesLevel(int level) {
		ArrayList<ArrayList<Integer>> nodes_indexes = new ArrayList<ArrayList<Integer>>();

		TIntArrayStack node_index = new TIntArrayStack();
		node_index.push(rootNodeId);
		
		while (node_index.size() > 0) {
			RTreeNode n = getNode(node_index.pop());

			if (n.getLevel() == level) {
				ArrayList<Integer> entries = getSubObjects(n);
				nodes_indexes.add(entries);
			}

			else if (n.getLevel() > level)
				for (int i = 0; i < n.entryCount; i++)
					node_index.push(n.ids[i]);
		}

		MBTools.debug("Level " + level + " Node size " + nodes_indexes.size(), false);
		return nodes_indexes;
	}

	private ArrayList<Integer> getSubObjects(RTreeNode node) {
		ArrayList<Integer> indexes = new ArrayList<Integer>();

		parents.clear();
		parentsEntry.clear();

		parents.push(node.nodeId);
		parentsEntry.push(-1);

		while (parents.size() > 0) {
			RTreeNode n = getNode(parents.peek());
			int startIndex = parentsEntry.peek() + 1;

			if (!n.isLeaf())
				for (int i = startIndex; i < n.entryCount; i++) {
					parents.push(n.ids[i]);
					parentsEntry.pop();
					parentsEntry.push(i);
					parentsEntry.push(-1);
				}

			else
				for (int i = 0; i < n.entryCount; i++) {
					indexes.add(n.ids[i]);
				}

			if (parents.size() > 0) {
				parents.pop();
				parentsEntry.pop();
			}
		}
		return indexes;
	}

	@Override
	public ArrayList<Integer> getIntersectingObjects(IObject3D object3d) {
		ArrayList<Integer> indexes = new ArrayList<Integer>();

		parents.clear();
		parentsEntry.clear();
		parents.push(rootNodeId);
		parentsEntry.push(-1);

		while (parents.size() > 0) {
			RTreeNode n = getNode(parents.peek());
			int startIndex = parentsEntry.peek() + 1;

			if (!n.isLeaf()) {
				// go through every entry in the index node to check
				// if it intersects the passed bounding box. if so, it could
				// contain entries that are contained.
				boolean intersects = false;
				for (int i = startIndex; i < n.entryCount; i++) {
					if (object3d.intersects(n.entries[i].getBoundingBox())) {
						parents.push(n.ids[i]);
						parentsEntry.pop();
						parentsEntry.push(i);
						parentsEntry.push(-1);
						intersects = true;
						break;
					}
				}
				if (intersects)
					continue;
			} else {
				for (int i = 0; i < n.entryCount; i++) {
					if (object3d.intersects(n.entries[i].getBoundingBox()))
						indexes.add(n.ids[i]);
				}
			}
			if (parents.size() > 0) {
				parents.pop();
				parentsEntry.pop();
			}
		}
		return indexes;
	}

	@Override
	public ArrayList<Integer> getContainingObjects(IObject3D object3d) {
		ArrayList<Integer> indexes = new ArrayList<Integer>();
		
		parents.clear();
		parentsEntry.clear();
		
		parents.push(rootNodeId);
		parentsEntry.push(-1);

		while (parents.size() > 0) {
			RTreeNode n = getNode(parents.peek());
			int startIndex = parentsEntry.peek() + 1;

			if (!n.isLeaf()) {
				// go through every entry in the index node to check
				// if it intersects the passed bounding box. if so, it could
				// contain entries that are contained.
				boolean intersects = false;
				for (int i = startIndex; i < n.entryCount; i++) {
					if (object3d.intersects(n.entries[i].getBoundingBox())) {
						parents.push(n.ids[i]);
						parentsEntry.pop();
						parentsEntry.push(i);
						parentsEntry.push(-1);
						intersects = true;
						break;
					}
				}
				if (intersects)
					continue;
			} else {
				for (int i = 0; i < n.entryCount; i++) {
					if (object3d.contains(n.entries[i].getBoundingBox()))
						indexes.add(n.ids[i]);
				}
			}
			parents.pop();
			parentsEntry.pop();
		}
		return indexes;
	}

	@Override
	public ArrayList<Integer> getObjects() {
		ArrayList<Integer> indexes = new ArrayList<Integer>();
		
		parents.clear();
		parentsEntry.clear();
		
		parents.push(rootNodeId);
		parentsEntry.push(-1);

		while (parents.size() > 0) {
			RTreeNode n = getNode(parents.peek());
			int startIndex = parentsEntry.peek() + 1;

			if (!n.isLeaf()) {
				for (int i = startIndex; i < n.entryCount; i++) {
					parents.push(n.ids[i]);
					parentsEntry.pop();
					parentsEntry.push(i);
					parentsEntry.push(-1);
				}
			} else {
				for (int i = 0; i < n.entryCount; i++)
					indexes.add(n.ids[i]);
			}
			if (parents.size() > 0) {
				parents.pop();
				parentsEntry.pop();
			}
		}
		return indexes;
	}
}
